#include "Calibrator.h"
#include "LaserAutoCalibrator.h"
#include "LaserManualCalibrator.h"
#include "CornerCalibrator.h"

#include <fstream> 
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <stdarg.h>

#define ESC 27
#define ENTER 10

// вывод текста
void Calibrator :: printText(IplImage* img, const int x, const int y, const char* text) const{
    // задаём точку для вывода текста
    CvPoint pt = cvPoint( x, y );
    // инициализация шрифта
    CvFont font;
    cvInitFont( &font, CV_FONT_HERSHEY_COMPLEX,1.0, 1.0, 0, 1, CV_AA);
    // используя шрифт выводим на картинку текст
    cvPutText(img, text, pt, &font, CV_RGB(0, 250, 0) );
}

void Calibrator :: calibrate(bool manual, const char* configFile){
    IplImage* img  = 0;

    cvNamedWindow("calibrate", 0);
    cvMoveWindow("calibrate", 0, 0);


    int imgCount = 0;      
    while(true){
        // получаем следующий кадр
        img = cvQueryFrame( capture );
        ++imgCount;
        if( !img ) {
            break;
        }
        int height    = img->height;
        int width     = img->width;
        printText (img, height/4, width*1/10, "Turn on laser.");
        printText (img, height/4, width*2/10, "Minimize me.");
        printText (img, height/4, width*3/10, "Hurry up!");
        cvShowImage( "calibrate", img );

        char c = cvWaitKey(33);

        if (c == ESC) {
            break;
        }else if(c == ENTER) {
            img = cvQueryFrame( capture );
            if( !img ) {
                break;
            }
            cvSaveImage("CalibrationImage.jpg", img);
            printf("The image has been saved.\n");
        }
        if (imgCount == 200){
            img = cvQueryFrame( capture );
            if( !img ) {
                break;
            }
            cvSaveImage("CalibrationImage.jpg", img);
            printf("The image has been saved.\n");
            break;
        }
    }
    cvDestroyWindow("calibrate");

    img = cvLoadImage("CalibrationImage.jpg",1);

    assert( img != 0 );

    if (manual == true){
        LaserManualCalibrator laserCalibrator(img);
        laserCalibrator.getParams(&Hmin, &Hmax, &Smin, &Smax, &Vmin, &Vmax);
    } else {
        LaserAutoCalibrator laserCalibrator (img);
        laserCalibrator.getParams(&Hmin, &Hmax, &Smin, &Smax, &Vmin, &Vmax);
    }

    CornerCalibrator cornerCalibr (img);
    cornerCalibr.getParams(&topLeftX, &topLeftY, &topRightX, &topRightY, &bottomLeftX, &bottomLeftY, &bottomRightX, &bottomRightY);

    std :: ofstream out(configFile);
    out << Hmin << " " << Hmax << " " << Smin << " " << Smax << " " << Vmin << " " << Vmax << "\n";
    out << topLeftX << " " << topLeftY << " " << topRightX << " " << topRightY << " " << bottomLeftX << " " << bottomLeftY << " " << bottomRightX << " " << bottomRightY << "\n";
    out.close();

    cvReleaseImage(&img);
}

void Calibrator :: setVideoStream (const char* videoStream, int cam_id){
    if (videoStream == NULL){
        //capture = cvCreateCameraCapture(CV_CAP_ANY);
        capture = cvCreateCameraCapture(cam_id);
        assert(capture);
    }else{
        //получаем информацию о видео-файле
        const char* filename = videoStream;
        capture = cvCreateFileCapture( filename );
    }
    printf("[i] press Esc for quit!\n\n");
}

Calibrator::Calibrator(const char* videoStream, const char* configFile, bool manual, int cam_id){
    setVideoStream(videoStream, cam_id);
    calibrate(manual, configFile);
}

Calibrator::~Calibrator(){
    cvDestroyAllWindows();
}

